#include "bsp_current.h"
#include "bsp_timer.h"
// #include "driver/mcpwm_prelude.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "driver/ledc.h"
#include "bsp_voice.h"
#include "bsp_param.h"
#include "bsp_magnetic.h"
#if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2
#define EXAMPLE_ADC_OUTPUT_TYPE             ADC_DIGI_OUTPUT_FORMAT_TYPE1
#define EXAMPLE_ADC_GET_CHANNEL(p_data)     ((p_data)->type1.channel)
#define EXAMPLE_ADC_GET_DATA(p_data)        ((p_data)->type1.data)
#else
#define EXAMPLE_ADC_OUTPUT_TYPE             ADC_DIGI_OUTPUT_FORMAT_TYPE2
#define EXAMPLE_ADC_GET_CHANNEL(p_data)     ((p_data)->type2.channel)
#define EXAMPLE_ADC_GET_DATA(p_data)        ((p_data)->type2.data)
#endif

static struct current_function *current_function_ptr = NULL;
const static uint32_t current_real_amp[TOTAL_CURRENT_INTENSITY_LEVEL + 1] = {0, 1, 2, 4, 6, 8, 10, 12, 14, 16};
SemaphoreHandle_t xSemaphore_timer2;


extern const struct current_function current_mode_1_func;
extern const struct current_function current_mode_2_func;
extern const struct current_function current_mode_3_func;
extern const struct current_function current_mode_4_func;
const static struct current_function *current_modes[CURRENT_MODE_MAX] = 
{
	&current_mode_1_func,
	&current_mode_2_func,
	&current_mode_3_func,
	&current_mode_4_func,
};

adc_continuous_handle_t adc_handle = NULL;
static void continuous_adc_init()
{

    adc_continuous_handle_cfg_t adc_config = {
        .max_store_buf_size = ADC_BUFFER_SIZE,
        .conv_frame_size = ADC_READ_LEN,
    };
    ESP_ERROR_CHECK(adc_continuous_new_handle(&adc_config, &adc_handle));

    adc_continuous_config_t dig_cfg = {
        .sample_freq_hz = SAMPLE_RATE,
        .conv_mode = ADC_CONV_SINGLE_UNIT_1,
        .format = EXAMPLE_ADC_OUTPUT_TYPE,
    };

    adc_digi_pattern_config_t adc_pattern[SOC_ADC_PATT_LEN_MAX] = {0};
    dig_cfg.pattern_num = 2;
    adc_pattern[0].atten = ADC_ATTEN_DB_11;
    adc_pattern[0].channel = TEAS_BREAK_CHANNEL;
    adc_pattern[0].unit = ADC_UNIT_1;
    adc_pattern[0].bit_width = SOC_ADC_DIGI_MAX_BITWIDTH;

    adc_pattern[1].atten = ADC_ATTEN_DB_11;
    adc_pattern[1].channel = CL_BREAK_CHANNEL;
    adc_pattern[1].unit = ADC_UNIT_1;
    adc_pattern[1].bit_width = SOC_ADC_DIGI_MAX_BITWIDTH;

    dig_cfg.adc_pattern = adc_pattern;
    adc_continuous_config(adc_handle, &dig_cfg);
    adc_continuous_start(adc_handle);

}
static uint8_t break_status_buffer[2] = {THERAPEUTIC_HEAD_CONNECT};
void elec_break_read()
{
    esp_err_t ret;
    uint32_t ret_num = 0;
    uint8_t result[ADC_READ_LEN] = {0};
    int i = 0;
    uint32_t data_dl = 0;
    uint32_t data_cl = 0;
    uint32_t data_dl_num = 0;
    uint32_t data_cl_num = 0;
    uint8_t break_type;
    ret = adc_continuous_read(adc_handle, result, ADC_READ_LEN, &ret_num, 0);
    if (ret == ESP_OK) {
        break_status_buffer[DL_BREAK] = THERAPEUTIC_HEAD_DISCONNECT;
        break_status_buffer[CL_BREAK] = THERAPEUTIC_HEAD_DISCONNECT;
        data_dl = 0;
        data_cl = 0;
        data_dl_num = 0;
        data_cl_num = 0;
        for (i = 0; i < ret_num; i += SOC_ADC_DIGI_RESULT_BYTES) {
            adc_digi_output_data_t *p = (void*)&result[i];
            break_type = EXAMPLE_ADC_GET_CHANNEL(p);
            if(break_type == 8){
                data_dl += EXAMPLE_ADC_GET_DATA(p);
                data_dl_num++;
            }else if(break_type == 9){
                data_cl += EXAMPLE_ADC_GET_DATA(p);
                data_cl_num++;
            }
  
        }
        data_dl = data_dl/data_dl_num;
        data_cl = data_cl/data_cl_num;
        // printf("\r\n DL_BREAK=%ld,DL_NUM=%ld,CL_BREAK=%ld,CL_NUM=%ld\r\n", data_dl,data_dl_num, data_cl,data_cl_num);
        if(data_dl >= 1000)
            break_status_buffer[DL_BREAK] = THERAPEUTIC_HEAD_CONNECT;

        if(data_cl >= 1500)
            break_status_buffer[CL_BREAK] = THERAPEUTIC_HEAD_CONNECT;


    }

}
/* 电极片监控任务 */
void elec_break_monitor_task() 
{
    uint8_t dl_monitor_time = 0;
    uint8_t cl_monitor_time = 0;
    uint8_t cl_old_qd = POWER_0;
    uint8_t dl_old_qd = POWER_0;
    sys_main_param_t* elec_break_task_param = Get_sys_param_pos();
    for(;;){
        vTaskDelay(10 / portTICK_PERIOD_MS);
        elec_break_read();
        if(elec_break_task_param->sys_status == SYS_RUN_NORMAL){
            cl_monitor_time++;
            dl_monitor_time++;
            if(break_status_buffer[DL_BREAK] == THERAPEUTIC_HEAD_CONNECT){
                dl_monitor_time = 0;
            }
            if(break_status_buffer[CL_BREAK] == THERAPEUTIC_HEAD_CONNECT){
                cl_monitor_time = 0;
            }  

            if(dl_old_qd != elec_break_task_param->sys_mode_type_buffer[MODE_DLQD]){
                dl_old_qd = elec_break_task_param->sys_mode_type_buffer[MODE_DLQD];
                dl_monitor_time = 0;
            }

            if(dl_monitor_time >= 50){
                dl_monitor_time = 0;
                if(elec_break_task_param->sys_mode_type_buffer[MODE_DLQD] != POWER_0)
                    elec_break_task_param->sys_error_type |= ERROR_TYPE_ELEC_BREAK;
                
            }

            if(cl_old_qd != elec_break_task_param->sys_mode_type_buffer[MODE_CCQD]){
                cl_old_qd = elec_break_task_param->sys_mode_type_buffer[MODE_CCQD];
                cl_monitor_time = 0;
            }
            if(cl_monitor_time >= 50){
                cl_monitor_time = 0;
                if(elec_break_task_param->sys_mode_type_buffer[MODE_CCQD] != POWER_0)
                    elec_break_task_param->sys_error_type |= ERROR_TYPE_CL_BREAK;
            }

        }else{
            dl_monitor_time = 0;
            cl_monitor_time = 0;
        }

    }
}

/* 电流强度调制管脚初始化 */
static void current_pwm_init()
{

    ledc_timer_config_t current_pwm_timer = {
        .speed_mode       = LEDC_LOW_SPEED_MODE,
        .duty_resolution  = LEDC_TIMER_8_BIT,
        .timer_num        = LEDC_TIMER_0,
        .freq_hz          = CURRENT_MCPWM_FREQUENCY, 
        .clk_cfg          = LEDC_AUTO_CLK
    };

    ESP_ERROR_CHECK(ledc_timer_config(&current_pwm_timer));
    // Prepare and then apply the LEDC PWM channel configuration
    ledc_channel_config_t current_pwm_channel = {
        .speed_mode     = LEDC_LOW_SPEED_MODE,
        .channel        = LEDC_CHANNEL_0,
        .timer_sel      = LEDC_TIMER_0,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = CURRENT_GPIO,
        .duty           = 0, // Set duty to 0%
        .hpoint         = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&current_pwm_channel));

}

static void current_pwm_duty_set(uint16_t duty)
{
    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty));
    ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
}



void current_initial(void)
{
	gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_DISABLE;
    //set as output mode
    io_conf.mode = GPIO_MODE_OUTPUT;
    //bit mask of the pins that you want to set,e.g.GPIO18/19
    io_conf.pin_bit_mask = CURRENT_OUTPUT_PIN_SEL;
    //disable pull-down mode
    io_conf.pull_down_en = 0;
    //disable pull-up mode
    io_conf.pull_up_en = 0;
    //configure GPIO with the given settings
    gpio_config(&io_conf);
    gpio_set_level(HBRIDGE_CTRL_0, 0);
    gpio_set_level(HBRIDGE_CTRL_1, 0);
    xSemaphore_timer2 = xSemaphoreCreateBinary();
    current_pwm_init();
    continuous_adc_init();
    
    xTaskCreate(elec_break_monitor_task, "elec_break_monitor_task", 4096, NULL, 9, NULL);


}


static void current_tick_service(void)
{
	if(current_function_ptr && current_function_ptr->tick_func)
		current_function_ptr->tick_func();
}

void current_output(uint32_t ua)
{
	uint32_t current_duty = 0;
	current_duty = ua * R_CY * CURRENT_MCPWM_TICKS / V_MAX;
	// if(!current_duty) 
	// 	current_duty = 1;	
	if(current_duty >= CURRENT_MCPWM_TICKS)
		current_duty = CURRENT_MCPWM_TICKS-1;

	current_pwm_duty_set(current_duty);

}

/* 转换电刺激强度 */
uint32_t current_trans_intensity(uint32_t level)
{
	if(level > TOTAL_CURRENT_INTENSITY_LEVEL) 
		level = TOTAL_CURRENT_INTENSITY_LEVEL;
	return current_real_amp[level];
}

/* 改变电刺激强度 */
void current_set_intensity(uint32_t level)
{
    if(current_function_ptr && current_function_ptr->change_elec)
	    current_function_ptr->change_elec(level);
}


/* 改变频率 */
void current_set_frequecy(uint32_t level)
{
	if(current_function_ptr && current_function_ptr->change_freq)
		current_function_ptr->change_freq(level);
}


/* 开始电刺激 */
void current_output_start(uint32_t mode, uint32_t freq_lvl, uint32_t mA_lvl, uint8_t music_vol)
{
	current_function_ptr = current_modes[mode-1];
	current_function_ptr->initial(freq_lvl, mA_lvl);
	current_function_ptr->start();
    xSemaphoreGive(xSemaphore_timer2);
	gptimer2_open();
    voice_output(music_vol);

}

void current_output_stop(void)
{
    BaseType_t err = pdFALSE;
    err = xSemaphoreTake(xSemaphore_timer2,0);
    if(err == pdPASS){
        gptimer2_close();
        current_function_ptr->stop();
        voice_close_output();
    }
	
}

extern void magnetic_tick_service(void);
IRAM_ATTR bool my_timer2_on_alarm_cb(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *user_ctx)
{
	current_tick_service();
    magnetic_tick_service();
    return pdTRUE;
}











